//
// Created by tong on 9/28/24.
//

#pragma once

#include <vector>
#include <array>
#include <memory>
#include <numeric>
#include <unordered_map>
#include <unordered_set>
#include <iostream>

#include "nanoflann.h"

//常量定义
constexpr double SMALL = 1e-12;
constexpr double PI = 3.14159265358979323846;


//直接使用Std命名空间
using std::pair;
using std::vector;
using std::shared_ptr;
using std::unique_ptr;
using std::max;
using std::min;
using std::array;
using std::unordered_map;
using std::unordered_set;
using std::abs;

// 自定义类型
//向量
struct Vec;
//角点位置
struct CornerPoint;
//插值点云
struct PointCloud;
//点
using Point=Vec;
//label
using Label = size_t;
//点数组
using PointList = vector<Point>;
//向量数组
using VecList = vector<Point>;
//double数组
using DoubleList = vector<double>;
//double二维数组
using DoubleListList = vector<DoubleList>;
//label数组
using LabelList = vector<Label>;
//label二维数组
using LabelListList = vector<LabelList>;
//int数组
using IntList = vector<int>;
//intPair
using IntPair = pair<int,int>;
//int二维数组
using IntListList = vector<IntList>;
//int三维数组
using IntListListList = vector<IntListList>;
//bool数组
using BoolList = vector<bool>;
//柱线角点对交线索引
using IntersectIndex = array<int,4>;
//柱线角点对交线索引数组
using IntersectIndexList = vector<IntersectIndex>;
//int哈希表
using IntSet = unordered_set<int>;
//label哈希表
using LabelSet = unordered_set<Label>;
//int到int的哈希表映射
template <typename key,typename Value>
using Map = unordered_map<key,Value>;

// using TetraStencil = array<size_t,4>;
// using TetraStencilList = vector<TetraStencil>;
// using TetraWeight = array<double,4>;
// using TetraWeightList = vector<TetraWeight>;
//角点数据类型
using CornerList = vector<CornerPoint>;
using CornerListList = vector<CornerList>;
using CornerListListList = vector<CornerListList>;
//KD树类型
using KNNResultType = nanoflann::KNNResultSet<double>;
using L2SimpleAdaptor = nanoflann::L2_Simple_Adaptor<double, PointCloud>;
using KDTreeL2Type = nanoflann::KDTreeSingleIndexAdaptor<L2SimpleAdaptor, PointCloud, 3>;




// 三维向量(点)
struct Vec {
    double x;
    double y;
    double z;
    inline double inner(const Vec& v2) const noexcept
    {
        const Vec& v1 = *this;

        return (v1.x*v2.x + v1.y*v2.y + v1.z*v2.z);
    }

    inline Vec cross(const Vec& v2) const noexcept
    {
        const Vec& v1 = *this;
        return Vec
        (
            (v1.y*v2.z - v1.z*v2.y),
            (v1.z*v2.x - v1.x*v2.z),
            (v1.x*v2.y - v1.y*v2.x)
        );
    };

    inline void operator+=(const Vec& v2) noexcept
    {
        x += v2.x;
        y += v2.y;
        z += v2.z;
    }

    inline void operator-=(const Vec& v2) noexcept
    {
        x -= v2.x;
        y -= v2.y;
        z -= v2.z;
    }

    template<typename T>
    inline void operator-=(const T a) noexcept
    {
        x -= a;
        y -= a;
        z -= a;
    }
    
    template<typename T>
    inline void operator+=(const T a) noexcept
    {
        x += a;
        y += a;
        z += a;
    }

    template<typename T>
    inline void operator*= (const T a) noexcept
    {
        x *= a;
        y *= a;
        z *= a;
    }

    template<typename T>
    inline void operator/= (const T a) noexcept
    {
        x /= a;
        y /= a;
        z /= a;
    }
    
    inline double mag() const noexcept
    {
        return sqrt(x*x + y*y + z*z);
    }

    Vec() = default;
    explicit Vec(const double x1, const double y2,const double z3) noexcept
        :x(x1),y(y2),z(z3){};
    Vec(const Vec& vec)=default;
    //Vec(const Vec&& vec)=default;
};

//Ref点云数据结构
struct PointCloud
{

    using coord_t = double;  //!< The type of each coordinate
    const PointList& pts_;
    const size_t size_;
    PointCloud(const PointList& pts, const size_t size)
        :pts_(pts),size_(size){}

    inline size_t kdtree_get_point_count() const { return size_; }

    inline double kdtree_get_pt(const size_t idx, const size_t dim) const
    {   
        if (dim == 0)
            return pts_[idx].x;
        else if (dim == 1)
            return pts_[idx].y;
        else
            return pts_[idx].z;
    }

    inline const Point &operator[](const size_t idx) const
    {
        return pts_[idx];
    }

    template <class BBOX>
    bool kdtree_get_bbox(BBOX& /* bb */) const
    {
        return false;
    }
};

//角点数据结构
struct CornerPoint {
    double zcoord;
    int cellIndex;
    int posI;
    int posJ;
    int posK;
    bool active;
    CornerPoint()=default;
    CornerPoint(const double zValue,int cIdx,int pI,int pJ,int pK,bool actnum)
    :zcoord(zValue),cellIndex(cIdx),posI(pI),posJ(pJ),posK(pK),active(actnum){};
};

// 重载点、向量运算符
inline Vec operator+(const Vec& v1, const Vec& v2)
{
    return Vec(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
}

inline Vec operator-(const Vec& v1, const Vec& v2)
{
    return Vec(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
}

inline Vec operator^(const Vec& v1, const Vec& v2)
{
    return v1.cross(v2);
}

inline double operator&(const Vec& v1, const Vec& v2)
{
    return v1.inner(v2);
}

template<typename T>
inline Vec operator+(const Vec& v1,  const T a)
{
    return Vec(v1.x + a, v1.y + a, v1.z + a);
}

template<typename T>
inline Vec operator-(const Vec& v1,  const T a)
{
    return Vec(v1.x - a, v1.y - a, v1.z - a);
}

template<typename T>
inline Vec operator*(const Vec& v1,  const T a)
{
    return Vec(v1.x * a, v1.y * a, v1.z * a);
}

template<typename T>
inline Vec operator/(const Vec& v1,  const T a)
{
    return Vec(v1.x / a, v1.y / a, v1.z / a);
}

inline Vec cross(const Vec& v1, const Vec& v2)
{
    return v1.cross(v2);
}

inline double mag(const Vec& v1)
{
    return v1.mag();
}

inline Vec operator*(const double a,const Vec& v1)
{
    return Vec(v1.x * a, v1.y * a, v1.z * a);
}

inline double PointPlaneDist(const Point& p, const Point& p1, const Point& p2, const Point& p3)
{
    const Vec n = (p2 - p1) ^ (p3 - p1);
    return std::abs((p - p1) & n) / n.mag();
}

inline double VecAngle(const Vec& v1, const Vec& v2){
    return std::acos((v1 & v2) / (v1.mag() * v2.mag()));
}



inline Point triCenter(const Point& p1, const Point& p2, const Point& p3)
{
    return (1.0 / 3.0) * Point((p1.x + p2.x + p3.x), (p1.y + p2.y + p3.y), (p1.z + p2.z + p3.z));
}

inline Vec triAreaNormal(const Point& p1, const Point& p2, const Point& p3)
{
    return 0.5*((p2 - p1) ^ (p3 - p1));
}

inline Point quadCenter(const Point& p1, const Point& p2, const Point& p3, const Point& p4)
{
    return 0.25 * Point((p1.x + p2.x + p3.x + p4.x), (p1.y + p2.y + p3.y + p4.y), (p1.z + p2.z + p3.z + p4.z));
}

inline Vec quadAreaNormal(const Point& p1, const Point& p2, const Point& p3, const Point& p4)
{
    return 0.5*((p2 - p1) ^ (p3 - p1)) + 0.5*((p3 - p1) ^ (p4 - p1));
}

inline double tetraVolume(const Point& p1, const Point& p2, const Point& p3, const Point& p4)
{
    return (1.0 / 6.0) * std::abs(((p2 - p1) & ((p3 - p1) ^ (p4 - p1))));
}

inline double pyramidVolume(const Point& p1, const Point& p2, const Point& p3, const Point& p4, const Point& p5)
{
    return tetraVolume(p1, p2, p3, p4) + tetraVolume(p1, p3, p4, p5);
}

inline Point tetraCenter(const Point &p1, const Point &p2, const Point &p3, const Point &p4)
{
    return 0.25 * (p1 + p2 + p3 + p4);
}

inline Point PointsCenter(const PointList &points, const IntList &indices)
{
    Point center(0.0, 0.0, 0.0);
    for (const auto &idx : indices)
    {
        center += points[idx];
    }
    center /= indices.size();
    return center;
}

// 判断点是否在四面体内部
inline bool pointInTetra(const Point &p, const Point &p1, const Point &p2, const Point &p3, const Point &p4)
{
    if (((p - p2) & triAreaNormal(p2, p3, p4)) > SMALL)
        return false;
    if (((p - p1) & triAreaNormal(p1, p4, p3)) > SMALL)
        return false;
    if (((p - p1) & triAreaNormal(p1, p2, p4)) > SMALL)
        return false;
    if (((p - p1) & triAreaNormal(p1, p3, p2)) > SMALL)
        return false;
    return true;
}

// 计算点在四面体内部的权重
// inline TetraWeight tetraWeights(const Point& p, const Point& p1, const Point& p2, const Point& p3, const Point& p4)
// {
//     const double vol = tetraVolume(p1, p2, p3, p4);
//     const double w1 = tetraVolume(p, p2, p3, p4) / vol;
//     const double w2 = tetraVolume(p, p1, p3, p4) / vol;
//     const double w3 = tetraVolume(p, p1, p2, p4) / vol;
//     const double w4 = tetraVolume(p, p1, p2, p3) / vol;
//     return {w1, w2, w3, w4};
// }

// 快排并且返回排序后的索引
template <typename T>
IntList sortIndex(const vector<T> &v) {
    IntList idx(v.size());
    std::iota(idx.begin(), idx.end(), 0);
    std::sort(idx.begin(), idx.end(), [&v](int i1, int i2) { return v[i1] < v[i2]; });
    return idx;
}

